/*
* @File Name AStar.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\prm\AStar.c
* @Author: Ruige_Lee
* @Date:   2019-07-27 14:40:45
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-07-30 20:59:24
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*/
/*
* @File Name AStar.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\prm\AStar.c
* @Author: 29505
* @Date:   2019-07-23 21:54:15
* @Last Modified by:   29505
* @Last Modified time: 2019-07-23 22:31:52
* @Email: 295054118@whut.edu.cn
*/

#include <stdint.h>
#include <string.h>

uint32_t* edgeStatePointer;
uint8_t poseUsed[2431] = {0};
uint8_t edgeUsed[16384] = {0};

extern uint16_t lookUpedgeTable[8192][2];
//使用一棵5叉树(忽略起点)保存生长组，直到生长叶子出现终点或者为空

//叶子用ping-pong链表临时保存，用于相交检测和生长

/*
生长起点和终点两棵树
*/

extern uint32_t poseNumActivateNextPoseNumEdgeNum( uint16_t poseNum, uint16_t nextPoseNum[6], uint16_t edgeNum[6]);
extern int32_t poseNum2Index(uint16_t poseNum, uint8_t xyz_index[3]);
uint32_t AStar_growGroup(uint16_t poseNum, uint16_t nextPoseNum[6], uint16_t edgeNum[6])
{
	poseNumActivateNextPoseNumEdgeNum( poseNum, nextPoseNum, edgeNum);

	for ( uint8_t i = 0; i < 6; i ++ )
	{
		if ( nextPoseNum[i] == 65535 ) //no pose there
		{
			continue;					//also no edge:edgeNum[i] = 65535;
		}

		uint16_t pNum = nextPoseNum[i];
//		uint16_t eNum = edgeNum[i];
		if ( poseUsed[pNum] == 1 )	// the pose is used
		{
			nextPoseNum[i] = 65535;
			edgeNum[i] = 65535;
			continue;
		}


		// visual obstacle
		uint8_t xyz_index[3];
		poseNum2Index(nextPoseNum[i], xyz_index);
		if ( (xyz_index[0] > 7) && (xyz_index[1] > 5) && (xyz_index[1] < 10) && (xyz_index[2] < 10))
		{
			nextPoseNum[i] = 65535;
			edgeNum[i] = 65535;
			continue;
		}
/*
		if ( 1 == collode(eNum) )
		{
			nextPoseNum[i] = 65535;
			edgeNum[i] = 65535;
		}
*/
	}

	return 0;
}

static uint16_t AStar_NextEdge(uint16_t nextPoseNum)
{
	uint16_t edgeNum[6];

	//attention ！！！ 对于同一条edge正向传播和反向传播方向应该是相反的，而且应该是唯一的
	poseNumActivateNextPoseNumEdgeNum(nextPoseNum, NULL, edgeNum);

	for ( uint8_t i = 0; i < 6; i ++ )
	{
		uint16_t eNum = edgeNum[i];

		if (( (i == 0) || (i == 2) || (i == 4)) && (edgeUsed[eNum] == 2 ))
		{
			return lookUpedgeTable[eNum][1];
		}

		else if (( (i == 1) || (i == 3) || (i == 5)) && (edgeUsed[eNum] == 1 ))

		{
			
			return lookUpedgeTable[eNum][0];
		}
	}
	return 65535;
}


static int32_t AStar_createEdge(uint16_t startPoseNum, uint16_t destPoseNum,uint16_t router[100],uint16_t *edgeCnt)	//生成路径
{
	


	uint16_t nextPoseNum = destPoseNum;

	memset(router, 0, sizeof(uint16_t) * 100);
	router[*edgeCnt] = destPoseNum;
	while(1)
	{
		
		nextPoseNum = AStar_NextEdge(nextPoseNum);
		if ( 65535 == nextPoseNum )	//error
		{
			return -1;
		}

	


		if (nextPoseNum != startPoseNum) 
		{
			(*edgeCnt) ++;
			router[*edgeCnt] = nextPoseNum;
			if ( *edgeCnt == 99 )	//edge overflow
			{
				return -1;
			}
			continue;
		}
		else //find out
		{
			break;
		}

	}




			

	return 0;
}

/**
 * [AS_excuse description] A star top module:
 * input start Pose
 * input dest Pose
 * output check point linklist
 * output searching state
 * @return [description]
 */

int32_t AStar_excuse(uint16_t startPoseNum, uint16_t destPoseNum, uint16_t router[100], uint16_t *edgeCnt)
{

	uint8_t poseActive[2][2431] = {{0}};//ping-pong activate

	
	uint16_t poseActiveCnt[2] = {0,0};

	uint8_t Ping = 0;
	uint8_t Pong = 1;

	//内存复位
	memset(poseUsed, 0, sizeof(uint8_t) * 2431);
	memset(poseActive, 0, sizeof(uint8_t) * 2 * 2431);
	memset(edgeUsed, 0, sizeof(uint8_t) * 16384);


	//assert(startPoseNum != destPoseNum);//强制起点不是终点

	poseUsed[startPoseNum] = 1;			//起点已用
	poseActive[Ping][startPoseNum] = 1;		//起点进入激活数组
	poseActiveCnt[Ping] = 1;
	
	while(1)
	{
		if ( poseActiveCnt[Ping] == 0 )
		{
			return -1;
		}


		//遍历激活数组
		poseActiveCnt[Pong] = 0;
		for ( uint16_t i = 0 ; i < 2431 ; i ++ )
		{
			uint16_t findCnt = 0;
			if ( poseActive[Ping][i] == 0)	//本次不激活
			{
				;
			} 
			else	//发现需要激活的Pose
			{
				uint16_t NextPoseNum[6] = {0};
				uint16_t edgeNum[6] = {0};
				AStar_growGroup(i, NextPoseNum, edgeNum);//一个pose将产生5个下一次激活pose

				for ( uint8_t j = 0; j < 6; j ++ )
				{
					uint16_t pNum = NextPoseNum[j];
					uint16_t eNum = edgeNum[j];
					if ( pNum != 65535 ) //如果是有效的
					{
						poseUsed[pNum] = 1;  //在已使用grid中标记
						poseActiveCnt[Pong] ++ ;
						poseActive[Pong][pNum] = 1;
						if ( (j == 0) || (j == 2) || (j == 4) )
						{
							edgeUsed[eNum] = 1;	//edge正向行进
						}
						else
						{
							edgeUsed[eNum] = 2; //edge逆向行进
						}
					}


				}
				
				poseActive[Ping][i] = 0; //本激活pose失去激活态
				
				
				

				findCnt ++;
				if ( findCnt == poseActiveCnt[Ping] )
				{
					break;
				}

				
			}
					
		}

		if ( poseActive[Pong][destPoseNum] != 1 )	//本次没有找到终点
		{
			;
		}
		else	//已经找到终点
		{
			AStar_createEdge(startPoseNum,destPoseNum, router, edgeCnt);	//生成路径
			return 0;
		}


		if ( Ping == 0 )
		{
			Ping = 1;
			Pong = 0;
		}
		else
		{
			Ping = 0;
			Pong = 1;
		}
	
	}


}
